#include <memory>
#include "modules/planning/scenarios/traffic_light_left_turn_waiting_zone/traffic_light_left_turn_waiting_zone.h"

namespace apollo {
namespace planning {

using apollo::hdmap::HDMapUtil;

bool TrafficLightLeftTurnWaitingZone::Init(std::shared_ptr<DependencyInjector> injector, const std::string& name) {
    if (!Scenario::Init(injector, name)) {
        AERROR << "failed to init scenario" << Name();
        return false;
    }

    if (!Scenario::LoadConfig<TrafficLightLeftTurnWaitingZoneConfig>(&context_.scenario_config)) {
        AERROR << "fail to get specific config of scenario " << Name();
        return false;
    }
    return true;
}

bool TrafficLightLeftTurnWaitingZone::IsTransferable(const Scenario* const other_scenario, const Frame& frame) {
    if (other_scenario == nullptr || frame.reference_line_info().empty()) {
        return false;
    }
    
    const auto& reference_line_info = frame.reference_line_info().front();
    const auto& first_encountered_overlaps = reference_line_info.FirstEncounteredOverlaps();
    const std::vector<hdmap::PathOverlap> traffic_light_overlaps
            = reference_line_info.reference_line().map_path().signal_overlaps();
    
    if (first_encountered_overlaps.empty()) {
        ADEBUG << "No first encountered overlaps found";
        return false;
    }
    
    hdmap::PathOverlap* traffic_sign_overlap = nullptr;
    for (const auto& overlap : first_encountered_overlaps) {
        if (overlap.first == ReferenceLineInfo::STOP_SIGN || overlap.first == ReferenceLineInfo::YIELD_SIGN) {
            return false;
        } else if (overlap.first == ReferenceLineInfo::SIGNAL) {
            traffic_sign_overlap = const_cast<hdmap::PathOverlap*>(&overlap.second);
            break;
        }
    }
    
    if (traffic_sign_overlap == nullptr) {
        ADEBUG << "No traffic signal overlap found in first encountered overlaps";
        return false;
    }
    
    const double start_check_distance = context_.scenario_config.start_traffic_light_scenario_distance();
    const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
    const double distance_to_signal = traffic_sign_overlap->start_s - adc_front_edge_s;
    
    if (distance_to_signal > start_check_distance) {
        ADEBUG << "Too far from traffic signal: " << distance_to_signal << " > " << start_check_distance;
        return false;
    }
    
    const auto& turn_type = reference_line_info.GetPathTurnType(traffic_sign_overlap->start_s);
    if (turn_type != hdmap::Lane::LEFT_TURN) {
        ADEBUG << "Not a left turn path, turn type: " << turn_type;
        return false;
    }
    
    auto* base_map = HDMapUtil::BaseMapPtr();
    
    // 放宽信号灯组距离限制，从2.0米增加到5.0米，提高检测成功率
    static constexpr double kTrafficLightGroupingMaxDist = 5.0;  // unit: m
    
    AINFO << "Checking left turn waiting zone scenario conditions:";
    AINFO << "  - Distance to signal: " << distance_to_signal << " m";
    AINFO << "  - Turn type: " << turn_type;
    AINFO << "  - Traffic light overlaps count: " << traffic_light_overlaps.size();
    
    for (const auto& overlap : traffic_light_overlaps) {
        const double dist = overlap.start_s - traffic_sign_overlap->start_s;
        if (fabs(dist) <= kTrafficLightGroupingMaxDist) {
            const auto& signal = base_map->GetSignalById(hdmap::MakeMapId(overlap.object_id))->signal();
            
            // 放宽条件：只要检测到左转箭头信号灯即可，不强制要求两个停止线
            if (signal.subsignal_size() > 0 && 
                signal.subsignal(0).type() == apollo::hdmap::Subsignal::ARROW_LEFT) {
                
                AINFO << "Left turn waiting zone scenario activated!";
                AINFO << "  - Signal ID: " << overlap.object_id;
                AINFO << "  - Distance between signals: " << dist << " m";
                AINFO << "  - Stop lines count: " << signal.stop_line_size();
                
                return true;
            }
        }
    }
    
    ADEBUG << "No suitable left turn arrow signal found within " << kTrafficLightGroupingMaxDist << "m";
    return false;
}

bool TrafficLightLeftTurnWaitingZone::Enter(Frame* frame) {
    const auto& reference_line_info = frame->reference_line_info().front();
    const auto& first_encountered_overlaps = reference_line_info.FirstEncounteredOverlaps();
    const std::vector<hdmap::PathOverlap> traffic_light_overlaps
            = reference_line_info.reference_line().map_path().signal_overlaps();
    hdmap::PathOverlap* nearest_traffic_light_overlap = nullptr;
    for (const auto& overlap : first_encountered_overlaps) {
        if (overlap.first == ReferenceLineInfo::SIGNAL) {
            nearest_traffic_light_overlap = const_cast<hdmap::PathOverlap*>(&overlap.second);
            break;
        }
    }
    std::vector<hdmap::PathOverlap> next_traffic_lights;
    auto* base_map = HDMapUtil::BaseMapPtr();
    std::string left_turn_signal_id;
    // 保持与 IsTransferable 函数中的距离限制一致
    static constexpr double kTrafficLightGroupingMaxDist = 5.0;  // unit: m
    for (const auto& overlap : traffic_light_overlaps) {
        const double dist = overlap.start_s - nearest_traffic_light_overlap->start_s;
        if (fabs(dist) <= kTrafficLightGroupingMaxDist) {
            const auto signal = base_map->GetSignalById(hdmap::MakeMapId(overlap.object_id));
            AINFO << signal->id().id() << "," << overlap.object_id;
            if (signal->signal().subsignal(0).type() == apollo::hdmap::Subsignal::ARROW_LEFT) {
                context_.left_turn_traffic_light_id = overlap.object_id;
                AINFO << "left_turn_signal_id" << context_.left_turn_traffic_light_id;
            } else {
                context_.forward_traffic_light_id = overlap.object_id;
                AINFO << "forward signal id" << context_.forward_traffic_light_id;
            }
        }
    }

    return true;
}

bool TrafficLightLeftTurnWaitingZone::Exit(Frame* frame) {
    context_.left_turn_traffic_light_id.clear();
    context_.forward_traffic_light_id.clear();
    auto done_traffic_light = injector_->planning_context()->mutable_planning_status()->mutable_traffic_light();
    done_traffic_light->mutable_done_traffic_light_overlap_id()->Clear();
    return true;
}

}  // namespace planning
}  // namespace apollo